#include "../include/LoopClosing.hpp"

namespace ORBSLAM
{
    LoopClosing::LoopClosing(KeyFrameDatabase *p_keyframe_database) : mb_finish_requested(false), mb_reset_requested(false), mb_reset_activemap_requested(false), mn_loop_coincidence_num(0), mn_merge_coincidence_num(0), mp_keyframe_database(p_keyframe_database), mb_fix_scale(false)
    {
    }

    LoopClosing::LoopClosing()
    {
    }

    LoopClosing::~LoopClosing()
    {
    }

    void LoopClosing::Set_param(Param *p_param)
    {
        mp_param = p_param;
        mp_matcher = new Matcher();
        mp_matcher->Set_matcher_parameters(50, 100, 0.6, true, 60);
    }

    void LoopClosing::Set_localmap(LocalMap *p_localmap)
    {
        mp_localmap = p_localmap;
    }

    void LoopClosing::Run()
    {
        mb_finished = false;

        while (1)
        {
            if (Check_new_keyframe_exist())
            {
                bool b_detected = Detect_common_region();

                if (b_detected)
                {
                    if (mb_merge_detected)
                    {
                        Mat mat_Rmw = mp_merge_matched_keyframe->Get_Rcw();
                        Mat mat_tmw = mp_merge_matched_keyframe->Get_tcw();
                        g2o::Sim3 sim3_Smw2(Converter::toMatrix3d(mat_Rmw), Converter::toVector3d(mat_tmw), 1.0);
                        Mat mat_Rcw = mp_current_keyframe->Get_Rcw();
                        Mat mat_tcw = mp_current_keyframe->Get_tcw();
                        g2o::Sim3 sim3_Scw1(Converter::toMatrix3d(mat_Rcw), Converter::toVector3d(mat_tcw), 1.0);

                        g2o::Sim3 sim3_Sw2c = msim3_merge_Slw.inverse();
                        g2o::Sim3 sim3_Sw1m = msim3_merge_Slw;

                        msim3_old_new = sim3_Sw2c * sim3_Scw1;
                    }
                    if (mb_loop_detected)
                    {
                        msim3_loop_Scw = msim3_loop_Slw;
                        mvp_loop_mpt_for_fusion = mvp_loop_window_mpt;
                        Correct_loop();
                        mp_loop_last_cur_keyframe->Set_erase();
                        mp_loop_matched_keyframe->Set_erase();
                        mn_loop_coincidence_num = 0;
                        mvp_loop_matched_mpt.clear();
                        mvp_loop_window_mpt.clear();
                        mn_loop_not_found_num = 0;
                        mb_loop_detected = false;
                    }
                }
            }
            Reset_if_requested();
            if (Check_finish())
            {
                break;
            }
            usleep(5000);
        }

        Set_finish();
    }

    void LoopClosing::Insert_keyframe(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_loop_queue);
        if (p_keyframe->mn_keyframe_id != 0)
        {
            mlp_loop_keyframe_queue.push_back(p_keyframe);
        }
    }

    bool LoopClosing::Check_finish()
    {
        unique_lock<mutex> lock(mmutex_finish);
        return mb_finish_requested;
    }

    void LoopClosing::Set_finish()
    {
        unique_lock<mutex> lock(mmutex_finish);
        mb_finished = true;
    }

    void LoopClosing::Reset_if_requested()
    {
        unique_lock<mutex> lock(mmutex_reset);
        if (mb_reset_requested)
        {
            mlp_loop_keyframe_queue.clear();
            mn_last_loop_keyframe_id = 0;
            mb_reset_requested = false;
            mb_reset_activemap_requested = false;
        }
        else if (mb_reset_activemap_requested)
        {
            for (auto it = mlp_loop_keyframe_queue.begin(); it != mlp_loop_keyframe_queue.end();)
            {
                KeyFrame *p_keyframei = *it;
                if (p_keyframei->Get_map() == mp_map_to_reset)
                {
                    it = mlp_loop_keyframe_queue.erase(it);
                }
                else
                {
                    ++it;
                }
            }
            mn_last_loop_keyframe_id = mp_mapatlas->Get_last_init_keyframe_id();
            mb_reset_activemap_requested = false;
        }
    }

    void LoopClosing::Correct_loop()
    {
        mp_localmap->Request_stop();
        mp_localmap->Empty_queue();
        if (Is_runing_gba())
        {
            unique_lock<mutex> lock(mmutex_gba);
            mb_stop_gba = true;
            mn_full_ba_num++;
            if (mp_thread_gba)
            {
                mp_thread_gba->detach();
                delete mp_thread_gba;
            }
        }

        while (!mp_localmap->Is_stoped())
        {
            usleep(1000);
        }

        mp_current_keyframe->Update_connections();

        mvp_current_connected_keyframe = mp_current_keyframe->Get_all_covisibility_keyframe();
        mvp_current_connected_keyframe.push_back(mp_current_keyframe);

        type_keyframe_and_pose corrected_sim3, non_corrected_sim3;
        corrected_sim3[mp_current_keyframe] = msim3_loop_Scw;

        Mat mat_Twc = mp_current_keyframe->Get_Twc();
        Map *p_loop_map = mp_current_keyframe->Get_map();

        {
            unique_lock<mutex> lock(p_loop_map->mmutex_mapupdate);

            for (auto p_keyframei : mvp_current_connected_keyframe)
            {
                Mat mat_Tiw = p_keyframei->Get_Tcw();
                if (p_keyframei != mp_current_keyframe)
                {
                    Mat mat_Tic = mat_Tiw * mat_Twc;
                    Mat mat_Ric = mat_Tic.rowRange(0, 3).colRange(0, 3);
                    Mat mat_tic = mat_Tic.rowRange(0, 3).col(3);

                    g2o::Sim3 sim3_Sic(Converter::toMatrix3d(mat_Ric), Converter::toVector3d(mat_tic), 1.0);
                    g2o::Sim3 sim3_Siw_corrected = sim3_Sic * msim3_loop_Scw;
                    corrected_sim3[p_keyframei] = sim3_Siw_corrected;
                }

                Mat mat_Riw = mat_Tiw.rowRange(0, 3).colRange(0, 3);
                Mat mat_tiw = mat_tiw.rowRange(0, 3).col(3);
                g2o::Sim3 sim3_Siw(Converter::toMatrix3d(mat_Riw), Converter::toVector3d(mat_tiw), 1.0);
                non_corrected_sim3[p_keyframei] = sim3_Siw;
            }

            for (auto keyframe_posei : corrected_sim3)
            {
                KeyFrame *p_keyframei = keyframe_posei.first;
                g2o::Sim3 sim3_Siw_corrected = keyframe_posei.second;
                g2o::Sim3 sim3_Swi_corrected = sim3_Siw_corrected.inverse();

                g2o::Sim3 sim3_Siw_non_corrected = non_corrected_sim3[p_keyframei];

                vector<MapPoint *> vp_keyframei_mpt = p_keyframei->Get_vect_mappoints();

                for (int i = 0; i < vp_keyframei_mpt.size(); i++)
                {
                    MapPoint *p_mpt = vp_keyframei_mpt[i];
                    if (!p_mpt || p_mpt->Is_bad())
                    {
                        continue;
                    }
                    if (p_mpt->mn_mark_corrected_by_keyframe == mp_current_keyframe->mn_keyframe_id)
                    {
                        continue;
                    }
                    Mat mat_p3dw = p_mpt->Get_world_pose();
                    Eigen::Matrix<double, 3, 1> eig_p3dw = Converter::toVector3d(mat_p3dw);
                    Eigen::Matrix<double, 3, 1> eig_p3dw_corrected = sim3_Swi_corrected.map(sim3_Siw_non_corrected.map(eig_p3dw));
                    Mat mat_p3dw_corrected = Converter::toCvMat(eig_p3dw_corrected);
                    p_mpt->Set_world_pose(mat_p3dw_corrected);
                    p_mpt->mn_mark_corrected_by_keyframe = mp_current_keyframe->mn_keyframe_id;
                    p_mpt->mn_mark_corrected_refkeyframe = p_keyframei->mn_keyframe_id;
                    p_mpt->Update_normal_and_depth();
                }

                Eigen::Matrix3d eig_R = sim3_Siw_corrected.rotation().toRotationMatrix();
                Eigen::Vector3d eig_t = sim3_Siw_corrected.translation();
                double s = sim3_Siw_corrected.scale();

                eig_t *= (1. / s);
                Mat mat_Tiw_corrected = Converter::toCvSE3(eig_R, eig_t);
                p_keyframei->Set_pose(mat_Tiw_corrected);
                p_keyframei->Update_connections();
            }
            p_loop_map->Increase_change_index();

            for (int i = 0; i < mvp_loop_matched_mpt.size(); i++)
            {
                if (mvp_loop_matched_mpt[i])
                {
                    MapPoint *p_loop_mpt = mvp_loop_matched_mpt[i];
                    MapPoint *p_cur_mpt = mp_current_keyframe->Get_mappoint(i);
                    if (p_cur_mpt)
                    {
                        p_cur_mpt->Replace(p_loop_mpt);
                    }
                    else
                    {
                        mp_current_keyframe->Add_mappoint(p_loop_mpt, i);
                        p_loop_mpt->Add_observation(mp_current_keyframe, i);
                        p_loop_mpt->Compute_distinctive_descriptors();
                    }
                }
            }
        }

        Search_and_fuse(corrected_sim3, mvp_loop_mpt_for_fusion);

        map<KeyFrame *, set<KeyFrame *>> map_loop_connection;
        for (auto p_keyframei : mvp_current_connected_keyframe)
        {
            vector<KeyFrame *> vp_keyframei_neighbor = p_keyframei->Get_all_covisibility_keyframe();
            p_keyframei->Update_connections();
            map_loop_connection[p_keyframei] = p_keyframei->Get_connect_keyframe();
            for (auto p_keyframej : vp_keyframei_neighbor)
            {
                map_loop_connection[p_keyframei].erase(p_keyframej);
            }
            for (auto p_keyframew : mvp_current_connected_keyframe)
            {
                map_loop_connection[p_keyframei].erase(p_keyframew);
            }
        }

        bool b_fix_scale = mb_fix_scale;

        Optimizer::Optimize_essential_graph(p_loop_map, mp_loop_matched_keyframe, mp_current_keyframe, non_corrected_sim3, corrected_sim3, map_loop_connection, b_fix_scale);

        mp_loop_matched_keyframe->Add_loopedge(mp_current_keyframe);
        mp_current_keyframe->Add_loopedge(mp_loop_matched_keyframe);

        mp_localmap->Release();

        mn_last_loop_keyframe_id = mp_current_keyframe->mn_keyframe_id;
    }

    void LoopClosing::Search_and_fuse(const type_keyframe_and_pose &corrected_map, vector<MapPoint *> &vp_mpt_for_fusion)
    {
        mp_matcher->Set_ratio(0.8);
        int n_total_replace = 0;

        for (auto keyframe_and_posei : corrected_map)
        {
            int n_replace_num = 0;
            KeyFrame *p_keyframei = keyframe_and_posei.first;
            Map *p_map = p_keyframei->Get_map();

            g2o::Sim3 sim3_Scw = keyframe_and_posei.second;
            Mat mat_Scw = Converter::toCvMat(sim3_Scw);

            vector<MapPoint *> vp_replace_mpt(vp_mpt_for_fusion.size(), static_cast<MapPoint *>(NULL));
            int n_fused_num = mp_matcher->Fuse(p_keyframei, mat_Scw, vp_mpt_for_fusion, 4, vp_replace_mpt);
            unique_lock<mutex> lock(p_map->mmutex_mapupdate);
            for (int i = 0; i < vp_mpt_for_fusion.size(); i++)
            {
                MapPoint *p_mpt_replace = vp_replace_mpt[i];
                if (p_mpt_replace)
                {
                    n_replace_num++;
                    p_mpt_replace->Replace(vp_mpt_for_fusion[i]);
                }
            }
            n_total_replace += n_replace_num;
        }
    }

    bool LoopClosing::Is_runing_gba()
    {
        unique_lock<mutex> lock(mmutex_gba);
        return mb_runing_gba;
    }

    bool LoopClosing::Check_new_keyframe_exist()
    {
        unique_lock<mutex> lock(mmutex_loop_queue);
        return (!mlp_loop_keyframe_queue.empty());
    }

    bool LoopClosing::Detect_common_region()
    {
        {
            unique_lock<mutex> lock(mmutex_loop_queue);
            mp_current_keyframe = mlp_loop_keyframe_queue.front();
            mlp_loop_keyframe_queue.pop_front();
            mp_current_keyframe->Set_not_erase();
            mp_current_keyframe->mb_current_place_recognition = true;
            mp_last_map = mp_current_keyframe->Get_map();
        }

        if (mp_last_map->Get_all_keyframe().size() < 12)
        {
            mp_keyframe_database->Add(mp_current_keyframe);
            mp_current_keyframe->Set_erase();
            return false;
        }

        bool b_loop_detected_in_keyframe = false;
        bool b_check_spatial = false;

        if (mn_loop_coincidence_num > 0)
        {
            b_check_spatial = true;
            Mat mat_Tcl = mp_current_keyframe->Get_Tcw() * mp_loop_last_cur_keyframe->Get_Twc();
            g2o::Sim3 sim3_Scl(Converter::toMatrix3d(mat_Tcl.rowRange(0, 3).colRange(0, 3)), Converter::toVector3d(mat_Tcl.rowRange(0, 3).col(3)), 1.0);
            g2o::Sim3 sim3_Scw = sim3_Scl * msim3_loop_Slw;
            int n_proj_matched_num = 0;
            vector<MapPoint *> vp_matched_mpt;
            bool b_common_region = Detect_and_refine_sim3_from_last_keyframe(mp_current_keyframe, mp_loop_matched_keyframe, sim3_Scw, n_proj_matched_num, mvp_loop_window_mpt, vp_matched_mpt);
            if (b_common_region)
            {
                b_loop_detected_in_keyframe = true;
                mn_loop_coincidence_num++;
                mp_loop_last_cur_keyframe->Set_erase();
                mp_loop_last_cur_keyframe = mp_current_keyframe;
                msim3_loop_Slw = sim3_Scw;
                mvp_loop_matched_mpt = vp_matched_mpt;
                mb_loop_detected = mn_loop_coincidence_num >= 3;
                mn_loop_not_found_num = 0;
            }
            else
            {
                b_loop_detected_in_keyframe = false;
                mn_loop_not_found_num++;
                if (mn_loop_not_found_num >= 2)
                {
                    mp_loop_last_cur_keyframe->Set_erase();
                    mp_loop_matched_keyframe->Set_erase();
                    mn_loop_coincidence_num = 0;
                    mvp_loop_matched_mpt.clear();
                    mvp_loop_window_mpt.clear();
                    mn_loop_not_found_num = 0;
                }
            }
        }

        bool b_merge_detected_in_keyframe = false;

        if (mn_merge_coincidence_num > 0)
        {
            Mat mat_Tcl = mp_current_keyframe->Get_Tcw() * mp_merge_last_cur_keyframe->Get_Twc();
            g2o::Sim3 sim3_Scl(Converter::toMatrix3d(mat_Tcl.rowRange(0, 3).colRange(0, 3)), Converter::toVector3d(mat_Tcl.rowRange(0, 3).col(3)), 1.0);
            g2o::Sim3 sim3_Scw = sim3_Scl * msim3_merge_Slw;
            int n_proj_matched_num = 0;
            vector<MapPoint *> vp_matched_mpt;
            bool b_common_region = Detect_and_refine_sim3_from_last_keyframe(mp_current_keyframe, mp_merge_matched_keyframe, sim3_Scw, n_proj_matched_num, mvp_merge_window_mpt, vp_matched_mpt);
            if (b_common_region)
            {
                b_merge_detected_in_keyframe = true;
                mn_merge_coincidence_num++;
                mp_merge_last_cur_keyframe->Set_erase();
                mp_merge_last_cur_keyframe = mp_current_keyframe;
                msim3_merge_Slw = sim3_Scw;
                mvp_merge_matched_mpt = vp_matched_mpt;
                mb_merge_detected = mn_merge_coincidence_num >= 3;
                // mn_merge_not_found_num = 0;
            }
            else
            {
                mb_merge_detected = false;
                b_merge_detected_in_keyframe = false;
                mn_merge_not_found_num++;
                if (mn_merge_not_found_num >= 2)
                {
                    mp_merge_last_cur_keyframe->Set_erase();
                    mp_merge_matched_keyframe->Set_erase();
                    mn_merge_coincidence_num = 0;
                    mvp_merge_matched_mpt.clear();
                    mvp_merge_window_mpt.clear();
                    mn_merge_not_found_num = 0;
                }
            }
        }

        if (mb_loop_detected || mb_merge_detected)
        {
            mp_keyframe_database->Add(mp_current_keyframe);
            return true;
        }

        vector<KeyFrame *> vp_merge_bow_cand, vp_loop_bow_cand;

        if (!b_loop_detected_in_keyframe || !b_merge_detected_in_keyframe)
        {
            mp_keyframe_database->Detect_nbest_candidates(mp_current_keyframe, vp_loop_bow_cand, vp_merge_bow_cand, 3);
        }

        if (1)
        {

            if (!b_loop_detected_in_keyframe && !vp_loop_bow_cand.empty())
            {
                mb_loop_detected = Detect_common_regions_from_bow(vp_loop_bow_cand, mp_loop_matched_keyframe, mp_loop_last_cur_keyframe, msim3_loop_Slw, mn_loop_coincidence_num, mvp_loop_window_mpt, mvp_loop_matched_mpt);
            }

            if (!b_merge_detected_in_keyframe && !vp_merge_bow_cand.empty())
            {
                mb_merge_detected = Detect_common_regions_from_bow(vp_merge_bow_cand, mp_merge_matched_keyframe, mp_merge_last_cur_keyframe, msim3_merge_Slw, mn_merge_coincidence_num, mvp_merge_window_mpt, mvp_merge_matched_mpt);
            }

            mp_keyframe_database->Add(mp_current_keyframe);

            if (mb_merge_detected || mb_loop_detected)
            {
                return true;
            }
            mp_current_keyframe->Set_erase();
            mp_current_keyframe->mb_current_place_recognition = false;
        }

        return false;
    }

    bool LoopClosing::Detect_common_regions_from_bow(vector<KeyFrame *> &vp_bow_cand, KeyFrame *p_matched_keyframe2, KeyFrame *p_last_current_keyframe, g2o::Sim3 &g2o_Scw, int &n_coincidence_num, vector<MapPoint *> &vp_mpt, vector<MapPoint *> &vp_matched_mpt)
    {
        int n_bow_match_th = 30;
        int n_bow_inline = 15;
        int n_sim3_inline = 20;
        int n_proj_match = 50;
        int n_proj_opt_match = 80;

        set<KeyFrame *> sp_connect_keyframe = mp_current_keyframe->Get_connect_keyframe();

        int n_covisible_num = 5;
        mp_matcher->Set_ratio(0.9);

        KeyFrame *p_best_match_keyframe;
        int n_best_match_reproj = 0;
        int n_best_coincidence_num = 0;
        g2o::Sim3 sim3_best_Scw;
        vector<MapPoint *> vp_best_mpt;
        vector<MapPoint *> vp_best_match_mpt;

        // int candidates_num = vp_bow_cand.size();

        for (auto p_keyframei : vp_bow_cand)
        {
            if (1)
            {
                Debuger::Show_two_image(mp_current_keyframe,p_keyframei);
            }
            if (!p_keyframei || p_keyframei->Is_bad())
            {
                continue;
            }
            vector<KeyFrame *> vp_window_best_cov = p_keyframei->Get_best_covisibility_keyframe(n_covisible_num);
            // vp_window_best_cov.push_back(vp_window_best_cov[0]);
            // vp_window_best_cov[0] = p_keyframei;
            vp_window_best_cov.insert(vp_window_best_cov.begin(), p_keyframei);

            vector<vector<MapPoint *>> vvp_match_mpt;
            vvp_match_mpt.resize(vp_window_best_cov.size());

            set<MapPoint *> sp_matched_mpt;
            int n_bow_match_num = 0;
            KeyFrame *p_most_bow_matche_keyframe = p_keyframei; // TODO
            int n_most_bow_match_num = 0;

            vector<MapPoint *> vp_matched_mpt1 = vector<MapPoint *>(mp_current_keyframe->Get_vect_mappoints().size(), static_cast<MapPoint *>(NULL));
            vector<KeyFrame *> vp_matched_mpt1_keyframe = vector<KeyFrame *>(mp_current_keyframe->Get_vect_mappoints().size(), static_cast<KeyFrame *>(NULL));

            for (int j = 0; j < vp_window_best_cov.size(); j++)
            {
                if (!vp_window_best_cov[j] || vp_window_best_cov[j]->Is_bad())
                {
                    continue;
                }
                int num = mp_matcher->Search_by_bow(mp_current_keyframe, vp_window_best_cov[j], vvp_match_mpt[j]);
                if (num > n_most_bow_match_num)
                {
                    n_most_bow_match_num = num;
                    p_most_bow_matche_keyframe = vp_window_best_cov[j];
                }
            }

            if (1)
            {

                for (int j = 0; j < vp_window_best_cov.size(); j++)
                {
                    for (int k = 0; k < vvp_match_mpt[j].size(); k++)
                    {
                        MapPoint *p_mpt = vvp_match_mpt[j][k];
                        if (!p_mpt || p_mpt->Is_bad())
                        {
                            continue;
                        }
                        if (sp_matched_mpt.find(p_mpt) == sp_matched_mpt.end())
                        {
                            sp_matched_mpt.insert(p_mpt);
                            n_bow_match_num++;
                            vp_matched_mpt1[k] = p_mpt;
                            vp_matched_mpt1_keyframe[k] = vp_window_best_cov[j];
                        }
                    }
                }

                if (n_bow_match_num >= n_bow_match_th)
                {
                    bool b_fix_scale = mb_fix_scale;
                    Sim3Solver solver = Sim3Solver(mp_current_keyframe, p_most_bow_matche_keyframe, vp_matched_mpt1, b_fix_scale, vp_matched_mpt1_keyframe);
                    solver.Set_parameters(0.99, n_bow_inline, 300);

                    bool b_nomore = false;
                    vector<bool> vb_inliner;
                    int n_inliner;
                    bool b_coverge = false;
                    Mat T_cm;
                    while (!b_coverge && !b_nomore)
                    {
                        T_cm = solver.Iterate(20, b_nomore, vb_inliner, n_inliner, b_coverge);
                    }

                    if (b_coverge)
                    {
                        vp_window_best_cov.clear();
                        vp_window_best_cov = p_most_bow_matche_keyframe->Get_best_covisibility_keyframe(n_covisible_num);
                        int n_initial_cov_num = vp_window_best_cov.size();
                        vp_window_best_cov.push_back(p_most_bow_matche_keyframe);
                        set<MapPoint *> sp_mpt_tmp;
                        vector<MapPoint *> vp_window_mpt;
                        vector<KeyFrame *> vp_window_mpt_keyframe;
                        for (auto p_keyframe2 : vp_window_best_cov)
                        {
                            for (auto p_mpt : p_keyframe2->Get_vect_mappoints())
                            {
                                if (!p_mpt || p_mpt->Is_bad())
                                {
                                    continue;
                                }
                                if (sp_matched_mpt.find(p_mpt) == sp_mpt_tmp.end())
                                {
                                    sp_mpt_tmp.insert(p_mpt);
                                    vp_window_mpt.push_back(p_mpt);
                                    vp_window_mpt_keyframe.push_back(p_keyframe2);
                                }
                            }
                        }

                        g2o::Sim3 g_Scm(Converter::toMatrix3d(solver.Get_estimated_rotation()), Converter::toVector3d(solver.Get_estimated_translation()), solver.Get_estimated_scale());
                        g2o::Sim3 g_Smw(Converter::toMatrix3d(p_most_bow_matche_keyframe->Get_Rcw()), Converter::toVector3d(p_most_bow_matche_keyframe->Get_tcw()), 1.0);
                        g2o::Sim3 g_Scw = g_Scm * g_Smw;

                        Mat m_Scw = Converter::toCvMat(g_Scw);

                        vector<MapPoint *> vp_matched_mpt_tmp;
                        vp_matched_mpt_tmp.resize(mp_current_keyframe->Get_vect_mappoints().size(), static_cast<MapPoint *>(NULL));
                        vector<KeyFrame *> vp_matched_keyframe_tmp;
                        vp_matched_keyframe_tmp.resize(mp_current_keyframe->Get_vect_mappoints().size(), static_cast<KeyFrame *>(NULL));
                        mp_matcher->Set_ratio(0.75);
                        int num_proj_matches = mp_matcher->Search_by_projection(mp_current_keyframe, m_Scw, vp_window_mpt, vp_window_mpt_keyframe, vp_matched_mpt_tmp, vp_matched_keyframe_tmp, 8, 1.5);
                        if (num_proj_matches >= n_proj_match)
                        {
                            Eigen::Matrix<double, 7, 7> mat_hessian77;
                            bool b_fix_scale = mb_fix_scale;
                            int num_opt_matches = Optimizer::Optimize_sim3(mp_current_keyframe, p_most_bow_matche_keyframe, vp_matched_mpt_tmp, g_Scm, 10, mb_fix_scale, mat_hessian77, true);
                            if (num_opt_matches >= n_sim3_inline)
                            {
                                g_Scw = g_Scm * g_Smw;
                                m_Scw = Converter::toCvMat(g_Scw);
                                vp_matched_mpt_tmp.resize(mp_current_keyframe->Get_vect_mappoints().size(), static_cast<MapPoint *>(NULL));
                                int n_opt_proj_matche_num = mp_matcher->Search_by_projection(mp_current_keyframe, m_Scw, vp_window_mpt, vp_matched_mpt_tmp, 5, 1.0);
                                if (n_opt_proj_matche_num >= n_proj_match)
                                {
                                    int n_checked_keyframe_num = 0;
                                    vector<KeyFrame *> vp_current_cov_keyframe = mp_current_keyframe->Get_best_covisibility_keyframe(n_covisible_num);
                                    int j = 0;
                                    while (n_checked_keyframe_num < 3 && j < vp_current_cov_keyframe.size())
                                    {
                                        KeyFrame *p_keyframe2 = vp_current_cov_keyframe[j];
                                        Mat mat_Tjc = p_keyframe2->Get_Tcw() * mp_current_keyframe->Get_Twc();
                                        g2o::Sim3 g_Sjc(Converter::toMatrix3d(mat_Tjc.rowRange(0, 3).colRange(0, 3)), Converter::toVector3d(mat_Tjc.rowRange(0, 3).col(3)), 1.0);
                                        g2o::Sim3 g_Sjw = g_Sjc * g_Scw;
                                        int n_projmatch_j = 0;
                                        vector<MapPoint *> vp_match_mpt_j;
                                        bool b_valid = Check_common_region_from_cov_keyframe(p_keyframe2, p_most_bow_matche_keyframe, g_Sjw, n_projmatch_j, vp_window_mpt, vp_match_mpt_j);
                                        if (b_valid)
                                        {
                                            n_checked_keyframe_num++;
                                        }
                                        j++;
                                    }
                                    if (n_best_match_reproj < n_opt_proj_matche_num)
                                    {
                                        n_best_match_reproj = n_opt_proj_matche_num;
                                        n_best_coincidence_num = n_checked_keyframe_num;
                                        p_best_match_keyframe = p_most_bow_matche_keyframe;
                                        sim3_best_Scw = g_Scw;
                                        vp_best_mpt = vp_window_mpt;
                                        vp_best_match_mpt = vp_matched_mpt_tmp;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }

        if (n_best_match_reproj > 0)
        {
            p_last_current_keyframe = mp_current_keyframe;
            n_coincidence_num = n_best_coincidence_num;
            p_matched_keyframe2 = p_best_match_keyframe;
            p_matched_keyframe2->Set_not_erase();
            g2o_Scw = sim3_best_Scw;
            vp_mpt = vp_best_mpt;
            vp_matched_mpt = vp_best_match_mpt;
            return n_coincidence_num >= 3;
        }
        return false;
    }

    bool LoopClosing::Check_common_region_from_cov_keyframe(KeyFrame *p_keyframe_j, KeyFrame *p_candidate_keyframe, g2o::Sim3 &g_Scw, int &n_proj_match_num, vector<MapPoint *> &vp_candidate_mpt, vector<MapPoint *> &vp_matched_mpt)
    {
        // set<MapPoint *> sp_already_matched_mpt(vp_matched_mpt.begin(), vp_matched_mpt.end());
        n_proj_match_num = Find_match_by_projection(p_keyframe_j, p_candidate_keyframe, g_Scw, vp_candidate_mpt, vp_matched_mpt);
        int n_proj_match_th = 30;
        if (n_proj_match_num >= n_proj_match_th)
        {
            return true;
        }
        return false;
    }

    int LoopClosing::Find_match_by_projection(KeyFrame *p_keyframe_j, KeyFrame *p_candidate_keyframe, g2o::Sim3 &g_Scw, vector<MapPoint *> &vp_window_mpt, vector<MapPoint *> &vp_matched_mpt)
    {
        int n_covisible_num = 5;
        vector<KeyFrame *> vp_candidate_cov_keyframe = p_candidate_keyframe->Get_best_covisibility_keyframe(n_covisible_num);
        int n_ini_cov = vp_candidate_cov_keyframe.size();
        vp_candidate_cov_keyframe.push_back(p_candidate_keyframe);

        set<KeyFrame *> sp_check_keyframe(vp_candidate_cov_keyframe.begin(), vp_candidate_cov_keyframe.end());
        set<KeyFrame *> sp_j_cov = p_keyframe_j->Get_connect_keyframe();

        for (int i = 0; i < n_ini_cov; i++)
        {
            vector<KeyFrame *> vp_keyframe_tmp = vp_candidate_cov_keyframe[i]->Get_best_covisibility_keyframe(n_covisible_num);
            int n_inserted = 0;
            int k = 0;
            while (k < vp_keyframe_tmp.size() && n_inserted < n_covisible_num)
            {
                if (sp_check_keyframe.find(vp_keyframe_tmp[k]) == sp_check_keyframe.end() && sp_j_cov.find(vp_keyframe_tmp[k]) == sp_j_cov.end())
                {
                    sp_check_keyframe.insert(vp_keyframe_tmp[k]);
                    ++n_inserted;
                }
                ++k;
            }
            vp_candidate_cov_keyframe.insert(vp_candidate_cov_keyframe.end(), vp_keyframe_tmp.begin(), vp_keyframe_tmp.end());
        }

        set<MapPoint *> sp_mpt_tmp;
        vp_window_mpt.clear();
        vp_matched_mpt.clear();

        for (auto p_keyframe : vp_candidate_cov_keyframe)
        {
            for (auto p_mpt : p_keyframe->Get_vect_mappoints())
            {
                if (!p_mpt || p_mpt->Is_bad())
                {
                    continue;
                }
                if (sp_mpt_tmp.find(p_mpt) == sp_mpt_tmp.end())
                {
                    sp_mpt_tmp.insert(p_mpt);
                    vp_window_mpt.push_back(p_mpt);
                }
            }
        }

        Mat mat_Scw = Converter::toCvMat(g_Scw);

        mp_matcher->Set_ratio(0.9);
        vp_matched_mpt.resize(p_keyframe_j->Get_vect_mappoints().size(), static_cast<MapPoint *>(NULL));
        int num_match = mp_matcher->Search_by_projection(p_keyframe_j, mat_Scw, vp_window_mpt, vp_matched_mpt, 3, 1.5);
        return num_match;
    }
    bool LoopClosing::Detect_and_refine_sim3_from_last_keyframe(KeyFrame *p_cur_keyframe, KeyFrame *p_matched_keyframe, g2o::Sim3 &sim3_Scw, int &n_proj_matched_num, vector<MapPoint *> &vp_window_mpt, vector<MapPoint *> &vp_matched_mpt)
    {
        // set<MapPoint *> sp_already_mapcted_mpt;
        n_proj_matched_num = Find_match_by_projection(p_cur_keyframe, p_matched_keyframe, sim3_Scw, vp_window_mpt, vp_matched_mpt);

        int n_proj_match_th = 30;
        int n_opt_proj_match_th = 50;
        int n_proj_match_rep_th = 100;

        if (n_proj_matched_num > n_proj_match_th)
        {
            Mat mat_Scw = Converter::toCvMat(sim3_Scw);
            Mat mat_Rwm = p_matched_keyframe->Get_Rwc();
            Mat mat_twm = p_matched_keyframe->Get_twc();
            g2o::Sim3 sim3_Swm(Converter::toMatrix3d(mat_Rwm), Converter::toVector3d(mat_twm), 1.0);
            g2o::Sim3 sim3_Scm = sim3_Scw * sim3_Swm;
            Eigen::Matrix<double, 7, 7> mat_hessian77;

            bool b_fix_scale = mb_fix_scale;
            int n_opt_proj_match = Optimizer::Optimize_sim3(p_cur_keyframe, p_matched_keyframe, vp_matched_mpt, sim3_Scm, 10, b_fix_scale, mat_hessian77, true);

            if (n_opt_proj_match > n_opt_proj_match_th)
            {
                sim3_Scw = sim3_Scm * sim3_Swm.inverse();
                int n_proj_match_rep = Find_match_by_projection(p_cur_keyframe, p_matched_keyframe, sim3_Scw, vp_window_mpt, vp_matched_mpt);
                if (n_proj_match_rep > n_proj_match_rep_th)
                {
                    return true;
                }
            }
        }
        return false;
    }

}